﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace WingbotDriver
{
    class PidErr
    {
        public double e2;       //pid的e，统一为设定值减去实测值
        private double e0;      //上上个e
        private double e1;      //上个e
        public double ec;       //e的变化率
        private double ec0;     // 上一个ec
        //public double ei;       //积分累加项
        private bool firstRun = true;
        public object plotlock = new object();
        private readonly int QueueLen = 5000;
        public Queue<double> eq = new Queue<double>();
        public Queue<double> ecq = new Queue<double>();

        public void Refresh(double err)
        {
            if (firstRun)
            {
                firstRun = false;
                e0 = err;
                e1 = err;
                e2 = err;
            }
            e0 = e1;
            e1 = e2;
            e2 = err;
            ec0 = ec;
            ec = e2 - e1;
            // ec = (e0 - 4*e1 + 3*e2)/2; //三点微分公式

            if (ec == 0) ec = ec0; //如果ec==0说明值没有刷新？

            lock (plotlock)
            {
                eq.Enqueue(e2);
                ecq.Enqueue(ec);

                if (eq.Count >= QueueLen) eq.Dequeue();
                if (ecq.Count >= QueueLen) ecq.Dequeue();
            }

        }
    }

    public class CommonPid
    {
        public double Kp;
        public double Ki;
        public double Kd;
        public double tol;             //容差，小于多少不再进行PID控制
        public double EMax = 1000;      //PID调控的最大值
        public double P;
        public double I;
        public double D;

        public CommonPid() { }

        public CommonPid(double p, double i, double d, double e = 0)
        {
            Kp = p;
            Ki = i;
            Kd = i;
            tol = e;
        }

        public double GetPIDSum()
        {
            double ret = P + I + D;
            if (Math.Abs(ret) > EMax) ret = Math.Sign(ret) * EMax;
            return ret;
        }

    };
}
